#include "protocol.h"
#include <string.h>
#include "error.h"
#include <stdarg.h>


RT_USED const _app_server __attribute__((section("app_server$a"))) _app_server_start = {0x0000, NULL};
RT_USED const _app_server __attribute__((section("app_server$z"))) _app_server_end = {0xffff, NULL};

void protocol_on_frame(_protocol_frame *frame)
{
    const _app_server *srv;
    for (srv = &_app_server_start; srv < &_app_server_end; ++srv)
    {
        if (srv->server_cmd == frame->cmd_code && srv->server)
        {
            srv->server(frame);
            return;
        }
    }
    // 可选：未找到服务时的默认处理
}

/* 协议栈初始化 */
void protocol_init(protocol_ctx_t ctx)
{
    memset(ctx, 0, sizeof(*ctx));
    ctx->state = PROTO_STATE_IDLE;
}

/* 协议栈状态机接收解析进程（建议循环调用或在接收中断中调用） */
void protocol_input_process(protocol_ctx_t ctx)
{
    uint8_t byte;
    while (ctx->recv_bytes && ctx->recv_bytes(&byte, 1) == 1)
    {
        switch (ctx->state)
        {
        case PROTO_STATE_IDLE:
            if (byte == PROTOCOL_FRAME_HEAD1)
                ctx->state = PROTO_STATE_HEAD1;
            break;
        case PROTO_STATE_HEAD1:
            if (byte == PROTOCOL_FRAME_HEAD2)
            {
                ctx->state = PROTO_STATE_EXEC0;
                ctx->check = PROTOCOL_FRAME_HEAD1 ^ PROTOCOL_FRAME_HEAD2;
            }
            else
                ctx->state = PROTO_STATE_IDLE;
            break;
        case PROTO_STATE_EXEC0:
            ctx->exec_code = ((uint16_t)byte) << 8;
            ctx->check ^= byte;
            ctx->state = PROTO_STATE_EXEC1;
            break;
        case PROTO_STATE_EXEC1:
            ctx->exec_code |= byte;
            ctx->check ^= byte;
            ctx->state = PROTO_STATE_MOD0;
            break;
        case PROTO_STATE_MOD0:
            ctx->module_id = ((uint16_t)byte) << 8;
            ctx->check ^= byte;
            ctx->state = PROTO_STATE_MOD1;
            break;
        case PROTO_STATE_MOD1:
            ctx->module_id |= byte;
            ctx->check ^= byte;
            ctx->state = PROTO_STATE_LEN0;
            break;
        case PROTO_STATE_LEN0:
            ctx->data_len = ((uint16_t)byte) << 8;
            ctx->check ^= byte;
            ctx->state = PROTO_STATE_LEN1;
            break;
        case PROTO_STATE_LEN1:
            ctx->data_len |= byte;
            ctx->check ^= byte;
            if (ctx->data_len > PROTOCOL_MAX_DATA_LEN)
                ctx->state = PROTO_STATE_IDLE; // 长度非法
            else if (ctx->data_len == 0)
                ctx->state = PROTO_STATE_CHECK;
            else
            {
                ctx->data_cnt = 0;
                ctx->state = PROTO_STATE_DATA;
            }
            break;
        case PROTO_STATE_DATA:
            ctx->data[ctx->data_cnt++] = byte;
            ctx->check ^= byte;
            if (ctx->data_cnt >= ctx->data_len)
                ctx->state = PROTO_STATE_CHECK;
            break;
        case PROTO_STATE_CHECK:
            if (ctx->check == byte)
                ctx->state = PROTO_STATE_TAIL1;
            else
                ctx->state = PROTO_STATE_IDLE; // 校验失败
            break;
        case PROTO_STATE_TAIL1:
            if (byte == PROTOCOL_FRAME_TAIL1)
                ctx->state = PROTO_STATE_TAIL2;
            else
                ctx->state = PROTO_STATE_IDLE;
            break;
        case PROTO_STATE_TAIL2:
            if (byte == PROTOCOL_FRAME_TAIL2)
            {
                // 完整帧，解析命令/状态/参数
                _protocol_frame frame;
                memset(&frame, 0, sizeof(frame));
                frame.ctx = *ctx;
                frame.exec_code = ctx->exec_code;
                frame.module_id = ctx->module_id;
                if (ctx->data_len >= 4)
                {
                    frame.cmd_code = ((uint16_t)ctx->data[0] << 8) | ctx->data[1];
                    uint16_t status = ((uint16_t)ctx->data[2] << 8) | ctx->data[3];
                    frame.status_code.para_len = (status >> 6) & 0x3FF; // 高10位
                    frame.status_code.state_len = status & 0x3F;        // 低6位
                    frame.data = &ctx->data[4];
                }
                else
                {
                    frame.cmd_code = 0;
                    frame.status_code.para_len = 0;
                    frame.status_code.state_len = 0;
                    frame.data = NULL;
                }
                protocol_on_frame(&frame);
            }
            ctx->state = PROTO_STATE_IDLE;
            break;
        default:
            ctx->state = PROTO_STATE_IDLE;
            break;
        }
    }
}

/**
 * @brief 组装结果帧（普通通讯，module_id高1位为0），并发送
 */

#if 0

void protocol_result(protocol_frame_t frame, uint8_t *data, size_t len)
{
    if (!frame || !frame->ctx.send_bytes) return;
    uint8_t buf[8 + PROTOCOL_MAX_DATA_LEN];
    size_t idx = 0;
    uint8_t check = 0;
    // 帧头
    buf[idx++] = PROTOCOL_FRAME_HEAD1;
    buf[idx++] = PROTOCOL_FRAME_HEAD2;
    // 执行编码
    buf[idx++] = (frame->exec_code >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = frame->exec_code & 0xFF;
    check ^= buf[idx-1];
    // 模块编号（高1位为0，普通通讯）
    uint16_t module_id = frame->module_id & 0x7FFF;
    buf[idx++] = (module_id >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = module_id & 0xFF;
    check ^= buf[idx-1];
    // 长度
    buf[idx++] = (len >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = len & 0xFF;
    check ^= buf[idx-1];
    // 数据
    for (size_t i = 0; i < len; i++)
    {
        buf[idx++] = data[i];
        check ^= data[i];
    }
    // 校验
    buf[idx++] = check;
    // 帧尾
    buf[idx++] = PROTOCOL_FRAME_TAIL1;
    buf[idx++] = PROTOCOL_FRAME_TAIL2;
    // 发送
    frame->ctx.send_bytes(buf, idx);
}
#else
void protocol_result(protocol_frame_t frame, uint8_t *data, size_t len)
{
    if (!frame || !frame->ctx.send_bytes)
        return;

    uint8_t buf[8 + PROTOCOL_MAX_DATA_LEN];
    uint8_t err_buf[ERROR_MAX_NUM * 4];
    uint16_t nErrNum = 0;
    size_t nLenErr = 0;
    size_t nLenData = len;

    // 检查并追加错误信息
    if (error_has_error())
    {
        error_analyze(err_buf, &nErrNum);
        nLenErr = 4 * nErrNum;
    }

    // 组装状态字：高10位为数据长度，低6位为错误长度
    uint16_t nStatus = ((nLenData & 0x3FF) << 6) | (nLenErr & 0x3F);

    // 组装data区：[cmd_code][status][原始数据][错误信息]
    // cmd_code: 2字节（高字节在前），status: 2字节（高字节在前）
    uint8_t data_field[PROTOCOL_MAX_DATA_LEN];
    if (nLenData > 0 && data != NULL)
        memcpy(data_field + 4, data, nLenData);
    if (nLenErr > 0)
        memcpy(data_field + 4 + nLenData, err_buf, nLenErr);

    // 写cmd_code和status
    data_field[0] = (frame->cmd_code >> 8) & 0xFF;
    data_field[1] = frame->cmd_code & 0xFF;
    data_field[2] = (nStatus >> 8) & 0xFF;
    data_field[3] = nStatus & 0xFF;

    size_t total_len = 4 + nLenData + nLenErr;

    // 组帧
    size_t idx = 0;
    uint8_t check = 0;
    buf[idx++] = PROTOCOL_FRAME_HEAD1;
    buf[idx++] = PROTOCOL_FRAME_HEAD2;
    buf[idx++] = (frame->exec_code >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = frame->exec_code & 0xFF;
    check ^= buf[idx - 1];
    uint16_t module_id = frame->module_id & 0x7FFF; // 普通通讯
    buf[idx++] = (module_id >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = module_id & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = (total_len >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = total_len & 0xFF;
    check ^= buf[idx - 1];
    for (size_t i = 0; i < total_len; i++)
    {
        buf[idx++] = data_field[i];
        check ^= data_field[i];
    }
    buf[idx++] = check;
    buf[idx++] = PROTOCOL_FRAME_TAIL1;
    buf[idx++] = PROTOCOL_FRAME_TAIL2;

    frame->ctx.send_bytes(buf, idx);
}
#endif

/**
 * @brief 组装主动上报帧（module_id高1位置1），并发送
 */
#if 0
void protocol_report(protocol_frame_t frame, uint8_t *data, size_t len)
{
    if (!frame || !frame->ctx.send_bytes) return;
    uint8_t buf[8 + PROTOCOL_MAX_DATA_LEN];
    size_t idx = 0;
    uint8_t check = 0;
    // 帧头
    buf[idx++] = PROTOCOL_FRAME_HEAD1;
    buf[idx++] = PROTOCOL_FRAME_HEAD2;
    // 执行编码
    buf[idx++] = (frame->exec_code >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = frame->exec_code & 0xFF;
    check ^= buf[idx-1];
    // 模块编号（高1位为1，主动返回）
    uint16_t module_id = frame->module_id | 0x8000;
    buf[idx++] = (module_id >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = module_id & 0xFF;
    check ^= buf[idx-1];
    // 长度
    buf[idx++] = (len >> 8) & 0xFF;
    check ^= buf[idx-1];
    buf[idx++] = len & 0xFF;
    check ^= buf[idx-1];
    // 数据
    for (size_t i = 0; i < len; i++)
    {
        buf[idx++] = data[i];
        check ^= data[i];
    }
    // 校验
    buf[idx++] = check;
    // 帧尾
    buf[idx++] = PROTOCOL_FRAME_TAIL1;
    buf[idx++] = PROTOCOL_FRAME_TAIL2;
    // 发送
    frame->ctx.send_bytes(buf, idx);
}

#else

void protocol_report(protocol_frame_t frame, uint8_t *data, size_t len)
{
    if (!frame || !frame->ctx.send_bytes)
        return;

    uint8_t buf[8 + PROTOCOL_MAX_DATA_LEN];
    uint8_t err_buf[ERROR_MAX_NUM * 4];
    uint16_t nErrNum = 0;
    size_t nLenErr = 0;
    size_t nLenData = len;

    // 检查并追加错误信息
    if (error_has_error())
    {
        error_analyze(err_buf, &nErrNum);
        nLenErr = 4 * nErrNum;
    }

    // 组装状态字：高10位为数据长度，低6位为错误长度
    uint16_t nStatus = ((nLenData & 0x3FF) << 6) | (nLenErr & 0x3F);

    // 组装data区：[cmd_code][status][原始数据][错误信息]
    uint8_t data_field[PROTOCOL_MAX_DATA_LEN];
    if (nLenData > 0 && data != NULL)
        memcpy(data_field + 4, data, nLenData);
    if (nLenErr > 0)
        memcpy(data_field + 4 + nLenData, err_buf, nLenErr);

    // 写cmd_code和status
    data_field[0] = (frame->cmd_code >> 8) & 0xFF;
    data_field[1] = frame->cmd_code & 0xFF;
    data_field[2] = (nStatus >> 8) & 0xFF;
    data_field[3] = nStatus & 0xFF;

    size_t total_len = 4 + nLenData + nLenErr;

    // 组帧
    size_t idx = 0;
    uint8_t check = 0;
    buf[idx++] = PROTOCOL_FRAME_HEAD1;
    buf[idx++] = PROTOCOL_FRAME_HEAD2;
    buf[idx++] = (frame->exec_code >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = frame->exec_code & 0xFF;
    check ^= buf[idx - 1];
    uint16_t module_id = frame->module_id | 0x8000; // 主动上报
    buf[idx++] = (module_id >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = module_id & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = (total_len >> 8) & 0xFF;
    check ^= buf[idx - 1];
    buf[idx++] = total_len & 0xFF;
    check ^= buf[idx - 1];
    for (size_t i = 0; i < total_len; i++)
    {
        buf[idx++] = data_field[i];
        check ^= data_field[i];
    }
    buf[idx++] = check;
    buf[idx++] = PROTOCOL_FRAME_TAIL1;
    buf[idx++] = PROTOCOL_FRAME_TAIL2;

    frame->ctx.send_bytes(buf, idx);
}
#endif